/*

  V 1.0 - 15/3/14  - Backend code for the robotics - same code master - will be overwritten
  
  Robotics Challenge - - all supporting code

  Also has a TestAll function you use to test all components are working

*/

#include <ArduinoRobot.h>


// New ping stuff - wont need library as the CPP is included in sketch

#include <NewPing.h>


#define TRIGGER_PIN  A1  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     A0  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
int cm;                  // Approximate distance

int lDetect = M1, rDetect = M3;             // Pins for obstacle sensors (remove rDetect connection when downloading sketch to Arduino)
#define SampleSize  10

#define SENSORON true
#define SENSOROFF false

boolean leftObstacleSensor();
boolean rightObstacleSensor();

// Set up the values for the sensors
boolean leftSensor=false;
boolean rightSensor=false;




// the setup routine runs once when you press reset:
// This must be here to set your pins for the motor
void setup()
{                
  // Just make sure the Obstacle sensors are ok.
  pinMode (lDetect, INPUT);
  pinMode (rDetect, INPUT);
 
  Serial.begin(115200);  // Start the Serial and make sure the debug says 115200 

 // initialize the robot
  Robot.begin();

  // open the serial port to send the information of what you are reading
  Serial.begin(9600);  

}

//
// the loop routine runs over and over again forever:
// 


void reverse(int wait, int vSpeed)
{
  Robot.motorsWrite(-vSpeed, -vSpeed);  // move forward
  Serial.println("Moving backward: " + String(vSpeed));
  delay(wait);
}

void forward(int wait, int vSpeed)
{
  Serial.println("Moving forward: " + String(vSpeed));
  Robot.motorsWrite(vSpeed, vSpeed);  // move forward

  delay(wait);
}

void rightSpin(int wait, int vSpeed)
{
  Serial.println("Spinning right");
  Robot.motorsWrite(vSpeed, -vSpeed);  // move forward
  delay(wait);
}

void leftSpin(int wait,int vSpeed)
{
  Serial.println("Spinning left");
  Robot.motorsWrite(-vSpeed, vSpeed);  // move forward
  delay(wait);

}

void diffSpeed(int wait, int lSpeed, int rSpeed)
{
  Serial.println("Set speed");
  Robot.motorsWrite(lSpeed, rSpeed);  // move forward
  delay(wait);
}

void halt(int wait)
{
  Serial.println("Stopping");
  Robot.motorsStop();   
  delay(wait);
}

void readSensors()
{
 
 
  leftObstacleSensor();
  rightObstacleSensor();
  
}


// Sensors Functions - use this if you have probelms


boolean leftObstacleSensor()
{
 
  if (digitalRead(lDetect)==0) // Test left sensor
  {
    Serial.println("Left Detected");
    leftSensor=true;
    return true;
  }
  leftSensor=false;
  return false;
}

boolean rightObstacleSensor()
{
 
  if (digitalRead(rDetect)==0) // Test left sensor
  {
    Serial.println("Right Detected");
    leftSensor=true;
    return true;
  }
  leftSensor=false;
  return false;

}



